/*
 * @Date: 2020-10-19 22:18:49
 * @LastEditors: Tonser
 * @LastEditTime: 2020-10-20 19:40:26
 * @FilePath: /swarm_ws/src/px4_gcs/src/MultiDaodanSimu.cpp
 * @Description: 多弹模拟程序
 */

#include "OffboardCore.h"
#include <boost/bind.hpp>
#include "ros/ros.h"


int main(int argc, char **argv) {
    ros::init(argc, argv, "offb_core");
    ros::NodeHandle n;
    OffboardCore control(n);
    control.load_traj();
    control.initRosSubPubManager(n);
    ros::spinOnce();
    ros::Rate loop_rate(control.rate);
    if (control.use_resample) {
        ROS_INFO_STREAM("ID: " << control.id << " resample data origin data size: " << control.traj.size()
                               << "sampled data size: " << control.resample_num);
        TrajManager<float>::resample(control.resample_num,control.traj);
    }
    control.mainLoop();
}
